#include <queue>
#include <set>
#include <stack>

using namespace std;

struct Node {
  bduVec3f* foot_pos; // pointer on bduVec3f : will be size 4
  int moving_foot; //number of the current moving foot
  float* center; //coordinates of the center of the robot : will be size 2 (used identify nodes, to avoid repeated nodes in A*)
  float h; // heuristic
  float g; // cost
  float f; // f=g+h is used to compare nodes
  Node* parent;
  SDSimulatorState* state;

  Node(){
     moving_foot = 0;
     h = 0;
     g = 0;
     f = 0;
  }
  /*~Node(){
     delete center;
     delete foot_pos;
  }*/
};

struct step;

float EuclidDist(float deltaX, float deltaY);

bool GoalTest(Node * testNode);

void FindPath(stack<step>* path, Node* currentNode, Node* startNode);

void SavePathToFile(stack<step> * path);
